#pragma once

#include "constants.hpp"
#include "EcosWrapper.hpp"
#include <Eigen/Dense>
using namespace Eigen;


class model_landing_3dof {


    const double TWR = 2.0;
    const double g = 9.81;
    const double rG = 12.0; // Radius of gyration
    const double rTB = 20.0; // Lever arm, distance: engines to CG
    const double rTF  = 25.0; // 压心位置与发动机的距离(目前没有考虑升力,没有使用)
    //质量大小100t,是单位质量
    const double C_D = 0.00/200e3; //等效阻力系数,包括密度，不考虑随攻角变化 C_D=1/2*rho*C_D*S/m,此时C_D为阻力系数,m为质量，S为参考面积

    const double max_gimbal_angle = 0.5;//摇摆角度，(maxdirection)指向角没有约束
    const double max_attitude_angle  = PI/6;
    const double max_glide_slope_angle = PI/3; 
    //z phi 加z和vz，不考虑滚转 考虑偏航摆角 考虑更多的约束 1.姿态角约束 2.下滑道约束 3.角速度约束 
    // double rx_init = 40;
    // double ry_init = 100;
    // double vx_init = -30;
    // double vy_init = 0;
    // double rx_init = -691;
    // double ry_init = 1710;
    // double rz_init = -30;
    // double vx_init = 155;
    // double vy_init = -235;
    // double vz_init = 5.6;
    double rx_init = 66;
    double ry_init = 300;
    double rz_init = -2;
    double vx_init = 7.3;
    double vy_init = -29;
    double vz_init = 0.58;
    double theta_init = 0;
    double dtheta_init = PI/3;
    double phi_init = 0;
    double dphi_init = 0;

    double rx_final = 0;
    double ry_final = 0;
    double rz_final = 0;
    double vx_final = 0;
    double vy_final = 0;
    double vz_final = 0;
    double theta_final = PI/2;
    double dtheta_final = 0;
    double phi_final = 0;
    double dphi_final = 0;

public:

    //static constexpr size_t n_states = 6;
    static constexpr size_t n_states = 10;// x y z vx vy vz theta dtheta phi dphi
    static constexpr size_t n_inputs = 3;//throttle gimbalAngle directionAngle
    static string get_name(){ return "model_landing_3dof"; }

    using StateVector   = Eigen::Matrix<double, n_states,        1>;
    using ControlVector = Eigen::Matrix<double, n_inputs,        1>;
    using StateMatrix   = Eigen::Matrix<double, n_states, n_states>;
    using ControlMatrix = Eigen::Matrix<double, n_states, n_inputs>;

    double total_time_guess() { return ry_init/vy_init; }

    void initialize(Eigen::Matrix<double, n_states, K> &X, Eigen::Matrix<double, n_inputs, K> &U);

    StateVector                ode(const StateVector &x, const ControlVector &u);
    StateMatrix     state_jacobian(const StateVector &x, const ControlVector &u);
    ControlMatrix control_jacobian(const StateVector &x, const ControlVector &u);

    void add_application_constraints(optimization_problem::SecondOrderConeProgram &socp,
                                                        Eigen::Matrix<double, n_states, K> &X0,
                                                        Eigen::Matrix<double, n_inputs, K> &U0); 
    //void add_application_constraints(optimization_problem::SecondOrderConeProgram &socp);

    StateVector get_random_state();
    ControlVector get_random_input();

};